#include "modules/drivers/lidar/orin_hesai/receiver/lidar_receiver.h"

#include <iomanip>
#include <string>

#include "cyber/common/global_data.h"
#include "cyber/cyber.h"

namespace apollo {
namespace drivers {
namespace lidar_hesai {

LidarReceiver* LidarReceiver::self = nullptr;
LidarReceiver::LidarReceiver() {};

bool LidarReceiver::Init(
    const std::shared_ptr<CircularQueueBuffer<LidarPacket>> packet_buffer,
    const uint32_t firing_data_port) {
  self = this;
  packet_buffer_ = packet_buffer;

  hesai_sdk_ = std::make_shared<HesaiSDK>();
  if (!hesai_sdk_->Init(firing_data_port)) {
    AERROR << "Hesai SDK init failed!";
    return false;
  }
  hesai_sdk_->SetDataCallBack(OnHesaiLidarData);
  AINFO << "Init HESAI_XT 32 sensor receiver.";

  return true;
}

void LidarReceiver::Start() {
  AWARN << "[thread_name]lidar_receiver";
  hesai_sdk_->Start();
}

void LidarReceiver::OnHesaiLidarData(LidarPacket* pkt) {
  self->hesai_sdk_->CheckHighTemperature(pkt->hesaiPacket);
  pkt->stamp = self->hesai_sdk_->GetPacketTimestamp(pkt->hesaiPacket);
  pkt->type = LidarType::PANDAR_XT_32;
  self->packet_buffer_->Insert(pkt);
}

}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo
